/*---------------------------------------------------------------------
 * File name: icm42688p.c
 *
 * Copyright (c) <2020-2022>, <ChenLong>
 *
 * All rights reserved.
 *
 * Author: ChenLong
 * Email: worldlong@foxmail.com
 *--------------------------------------------------------------------*/
#include "icm42688p.h"
#include "xmath/xmath.h"
#if defined(USING_RT_THREAD)
#include "rtthread.h" 
#endif
/*
*/
void ICM42688P::write_reg(uint8_t addr, uint8_t data)
{
#if defined(USING_RT_THREAD)
  rt_enter_critical();
#endif
  
  uint8_t tx[2]={(uint8_t)(addr&0x7f), data};
  uint8_t rx[2];
  for(int i=0; i<_ncs; i++) {
    _cs->write(0);
  }
  _spi->transfer(tx, rx, 2);
  
  for(int i=0; i<_ncs; i++) {
    _cs->write(1);
  }
 
#if defined(USING_RT_THREAD)
  rt_exit_critical();
#endif
}
/*
*/
uint8_t ICM42688P::read_reg(uint8_t addr)
{
#if defined(USING_RT_THREAD)
  rt_enter_critical();
#endif
  uint8_t tx[2]={(uint8_t)(addr|0x80), 0x00};
  uint8_t rx[2];
  for(int i=0; i<_ncs; i++) {
    _cs->write(0);
  }
  _spi->transfer(tx, rx, 2);
  
  for(int i=0; i<_ncs; i++) {
    _cs->write(1);
  }
  
#if defined(USING_RT_THREAD) 
  rt_exit_critical();
#endif
  return rx[1];
}
/*
*/
bool ICM42688P::write_reg_with_check(uint8_t addr, uint8_t data)
{
  write_reg(addr, data);
  return (data == read_reg(addr));
}
/*
number <= 8
*/
int ICM42688P::read_bytes(uint8_t *data, uint8_t addr, int number)
{
  if(number > 16) {
    number = 16;
  }
  
#if defined(USING_RT_THREAD)
  rt_enter_critical();
#endif
  uint8_t tx[17]={(uint8_t)(addr|0x80), 0x00};
  uint8_t rx[17];

  for(int i=0; i<_ncs; i++) {
    _cs->write(0);
  }
  _spi->transfer(tx, rx, number + 1);
  
  for(int i=0; i<_ncs; i++) {
    _cs->write(1);
  }
  
  if(data) {
    memcpy(data, rx + 1, number);
  }
  
#if defined(USING_RT_THREAD) 
  rt_exit_critical();
#endif
  return number;
}
/*
*/
void ICM42688P::reset()
{
  write_reg(ICM42688P_REG_BANK_SEL, 0); //change to bank0
  write_reg(ICM42688P_DEVICE_CONFIG, 0x01);
}
/*
*/
bool ICM42688P::check()
{
  write_reg(ICM42688P_REG_BANK_SEL, 0); //change to bank0
  return (read_reg(ICM42688P_WHO_AM_I) == ICM42688P_WHO_AM_I_DATA);
}
/*
*/
bool ICM42688P::config(uint8_t *cfg[2], int size)
{
  for(int i=0; i<size; i++) {
    if(!write_reg_with_check(cfg[i][0], cfg[i][1])) {
      return false;
    }
  }
  return true;
}
/*
*/
bool ICM42688P::config(int accel_fs, int gyro_fs, int odr)
{
  uint8_t odr_cfg;
  uint8_t fs_cfg;
  /* When reset, just config some of bank0 registers. 
     All other regesters just keep the default value */
  //reset();
  
  /* Int default config:Pulsed mode and Active low */
  //if(!write_reg_with_check(ICM42688P_INT_CONFIG, 0x00)) {
  //  return false;
  //}
  
  /* Bypass Mode (default) */
  //if(!write_reg_with_check(ICM42688P_FIFO_CONFIG, 0x00)) {
  //  return false;
  //}
  
  /* Config FS and ODR */
  if(odr == 25) {
    odr_cfg = 10;
  } else if(odr == 50) {
    odr_cfg = 9;
  } else if(odr == 100) {
    odr_cfg = 8;
  } else if(odr == 200) {
    odr_cfg = 7;
  }else if(odr == 1000) {
    odr_cfg = 6;
  } else if(odr == 2000) {
    odr_cfg = 5;
  } else if(odr == 4000) {
    odr_cfg = 4;
  } else if(odr == 8000) {
    odr_cfg = 3;
  } else if(odr == 16000) {
    odr_cfg = 2;
  } else if(odr == 32000) {
    odr_cfg = 1;
  } else { //odr = 500Hz
     odr_cfg = 15;
  }
  /* gyro FS */
  if(gyro_fs == 125) {
    fs_cfg = 4 << 5;
    gyro_scale = 1.0f / 262.0f;
  } else if(gyro_fs == 250) {
    fs_cfg = 3 << 5;
    gyro_scale = 1.0f / 131.0f;
  } else if(gyro_fs == 500) {
    fs_cfg = 2 << 5;
    gyro_scale = 1.0f / 65.5f;
  } else if(gyro_fs == 1000) {
    fs_cfg = 1 << 5;
    gyro_scale = 1.0f / 32.8f;
  } else { //gyro_fs = 2000
    fs_cfg = 0; 
    gyro_scale = 1.0f / 16.4f;
  }
  if(!write_reg_with_check(ICM42688P_GYRO_CONFIG0, fs_cfg|odr_cfg)) {
    return false;
  }
     
  /* accel FS */
  if(accel_fs == 2) {
    fs_cfg = 3 << 5;
    accel_scale = 1.0f / 16384.0f;
  } else if(accel_fs == 4) {
    fs_cfg = 2 << 5;
    accel_scale = 1.0f / 8192.0f;
  } else if(accel_fs == 8) {
    fs_cfg = 1 << 5;
    accel_scale = 1.0f / 4096.0f;
  } else { //accel_fs = 16
    fs_cfg = 0;  
    accel_scale = 1.0f / 2048.0f;
  }
  if(!write_reg_with_check(ICM42688P_ACCEL_CONFIG0, fs_cfg|odr_cfg)) {
    return false;
  }
  
  /* Bandwidth = max(400Hz, ODR)/4 (default)  */
  //if(!write_reg_with_check(ICM42688P_GYRO_ACCEL_CONFIG0, 0x11)) {
  //  return false;
  //}
  
  /* UI data ready interrupt routed to INT1  */
  if(!write_reg_with_check(ICM42688P_INT_SOURCE0, 0x08)) {
    return false;
  }
     
  /* Enbale control: in Low Noise (LN) Mode  */
  if(!write_reg_with_check(ICM42688P_PWR_MGMT0, 0x0F)) {
    return false;
  }
  
  gyro_scale *= DEG2RAD;
  
  return true;
}
/*
*/
void ICM42688P:: read_raw(int16_t *temp, int16_t *accel, int16_t *gyro)
{
  uint8_t data[14];
  read_data(data);
  *temp = (data[0] << 8) + data[1];
  accel[0] = (data[2] << 8) + data[3];
  accel[1] = (data[4] << 8) + data[5];
  accel[2] = (data[6] << 8) + data[7];
  gyro[0] = (data[8] << 8) + data[9];
  gyro[1] = (data[10] << 8) + data[11];
  gyro[2] = (data[12] << 8) + data[13];
}
  
